//
// Created by luozj on 3/4/24.
//
#include "kinematics.h"

namespace qy {

    Eigen::Matrix4d dhFormMat(const DH &dh) {
        Eigen::Matrix4d mat = Eigen::Matrix4d::Identity();
        mat << cos(dh.theta), -sin(dh.theta), 0, dh.a,
                sin(dh.theta) * cos(dh.alpha), cos(dh.theta) * cos(dh.alpha), -sin(dh.alpha), -sin(dh.alpha) * dh.d,
                sin(dh.theta) * sin(dh.alpha), cos(dh.theta) * sin(dh.alpha), cos(dh.alpha), cos(dh.alpha) * dh.d,
                0, 0, 0, 1;
        return mat;
    }

    Eigen::MatrixXd FKinSpaceDH(const Eigen::VectorXd &thetalist) {
        /**左臂dh参数
         *                   alpha    a    d    theta   offset
         * shoulder_pan      0        0  0.08589    theta0   0
         * shoulder_tilt     -pi/2    0    0    theta1   0
         * shoulder_roll     pi/2     0  0.1654    theta2   -pi/2
         * elbow             pi/2     0  -0.010103 theta3  0
         * wrist_roll       -pi/2     0  0.210455  theta4  pi/2
         * wrist_flex       -pi/2     0   0      theta5   -pi/2
         */

        DH shoulder_pan(0, 0, 0, thetalist(0), 0);
        DH shoulder_tilt(-M_PI_2, 0, 0, thetalist(1), 0);
        DH shoulder_roll(M_PI_2, 0, 0.1654, thetalist(2), -M_PI_2);
        DH elbow(M_PI_2, 0, -0.010103, thetalist(3), 0);
        DH wrist_roll(-M_PI_2, 0, 0.210455, thetalist(4), M_PI_2);
        DH wrist_flex(-M_PI_2, 0, 0, thetalist(5), -M_PI_2);
        Eigen::MatrixXd result =
                dhFormMat(shoulder_pan) * dhFormMat(shoulder_tilt) * dhFormMat(shoulder_roll) * dhFormMat(elbow) *
                dhFormMat(wrist_roll) * dhFormMat(wrist_flex);
        return result;
    }
}